#include <memory>
#include "geometry_msgs/msg/transform_stamped.hpp" // 提供消息接口
#include "rclcpp/rclcpp.hpp"
#include "tf2/utils.h"                             // 将四元数转换为欧拉角
#include "tf2/LinearMath/Quaternion.h"             // 提供四元数接口
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" // 提供转换接口
#include "tf2_ros/transform_listener.h"            // 提供监听器接口
#include "tf2_ros/buffer.h"                        // 提供缓冲区接口
#include <chrono>                                  // 时间相关头文件

using namespace std::chrono_literals;

class TFListenerNode : public rclcpp::Node
{
public:
    TFListenerNode() : Node("tf_listener")
    {
        // 创建缓冲区
        tf_buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
        // 创建监听器
        tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_, this);
        // 定时器每500毫秒调用一次
        timer_ = this->create_wall_timer(5s, std::bind(&TFListenerNode::getTransform, this));
    }

    void getTransform()
    {
        // 获取当前时间
        auto now = this->get_clock()->now();
        // 查询变换
        try
        {
            // 等待变换可用
            const auto transform = tf_buffer_->lookupTransform("base_link", "target_point", this->get_clock()->now(),
                                                               rclcpp::Duration::from_seconds(1.0));

            // 转换结果及输出
            const auto &translation = transform.transform.translation;
            const auto &rotation = transform.transform.rotation;
            double roll, pitch, yaw;
            tf2::getEulerYPR(rotation, yaw, pitch, roll);

            RCLCPP_INFO(this->get_logger(), "平移分量:(%f, %f, %f)", translation.x, translation.y, translation.z);
            RCLCPP_INFO(this->get_logger(), "旋转分量:(%f, %f, %f)", roll, pitch, yaw);
        }
        catch (const tf2::TransformException &ex)
        {
            RCLCPP_WARN(this->get_logger(), "异常: %s", ex.what()); // 处理异常
        }
    }

private:
    std::shared_ptr<tf2_ros::Buffer> tf_buffer_;              // 缓冲区
    std::shared_ptr<tf2_ros::TransformListener> tf_listener_; // 监听器
    rclcpp::TimerBase::SharedPtr timer_;                      // 定时器
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<TFListenerNode>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}
